/*
 #
 #  Files       : SymMatrix.h
 #                ( C++ header file )
 #
 #  Description : The SmallMatrix Library
 #                ( http://code.google.com/p/smallmatrix )
 #
 #  Copyright   : Olivier Juan
 #                ( http://www.mas.ecp.fr/vision/Personnel/juan/ )
 #
 #  License     : CeCILL-C
 #                ( http://www.cecill.info/licences/Licence_CeCILL-C_V1-en.html )
 #
 #  This software is governed by the CeCILL-C license under French law and
 #  abiding by the rules of distribution of free software.  You can  use,
 #  modify and or redistribute the software under the terms of the CeCILL-C
 #  license as circulated by CEA, CNRS and INRIA at the following URL
 #  "http://www.cecill.info".
 #
 #  As a counterpart to the access to the source code and  rights to copy,
 #  modify and redistribute granted by the license, users are provided only
 #  with a limited warranty  and the software's author,  the holder of the
 #  economic rights,  and the successive licensors  have only  limited
 #  liability.
 #
 #  In this respect, the user's attention is drawn to the risks associated
 #  with loading,  using,  modifying and/or developing or reproducing the
 #  software by the user in light of its specific status of free software,
 #  that may mean  that it is complicated to manipulate,  and  that  also
 #  therefore means  that it is reserved for developers  and  experienced
 #  professionals having in-depth computer knowledge. Users are therefore
 #  encouraged to load and test the software's suitability as regards their
 #  requirements in conditions enabling the security of their systems and/or
 #  data to be ensured and,  more generally, to use and operate it in the
 #  same conditions as regards security.
 #
 #  The fact that you are presently reading this means that you have had
 #  knowledge of the CeCILL-C license and that you accept its terms.
 #
*/

#ifndef _SYMMATRIX_H
#define _SYMMATRIX_H

#if USE_MKL
#include <mkl.h>
#elif USE_ACML
#include <acml.h>
#endif

#include <SmallMatrix/SmallMatrix.h>
#include <SmallMatrix/Vecteur.h>

NAMESPACE_BEGIN(SmallMatrix)

template <int N> class SymMatrix : public Matrix_Base<SymMatrix<N>,(((N+1)*N)>>1),N,N>
{
public:
	SymMatrix():Matrix_Base<SymMatrix<N>,(((N+1)*N)>>1),N,N>(){}
	SymMatrix(const reel &v):Matrix_Base<SymMatrix<N>,(((N+1)*N)>>1),N,N>(v){}
	SymMatrix(const reel tab[(((N+1)*N)>>1)]):Matrix_Base<SymMatrix<N>,(((N+1)*N)>>1),N,N>(tab){}
	SymMatrix(const Vecteur<N>& v) {
		SymMatrix<N>& ref=*this;
		for (int i=0;i<N;i++)
			for (int j=i;j<N;j++)
                ref(i,j)=v(i,0)*v(j,0);
	}
	template <typename TT,int SSIZE> SymMatrix(const Matrix_Base<TT,SSIZE,N,N>& copy):Matrix_Base<SymMatrix<N>,(((N+1)*N)>>1),N,N>(copy){}
	inline reel& operator()(const int idxm,const int idxn);
	inline const reel& operator()(const int idxm,const int idxn)const;
	template <typename T1,int SIZE1,int MN,typename T2,int SIZE2> inline SymMatrix<N>& prod(const Matrix_Base<T1,SIZE1,N,MN>& M1,const Matrix_Base<T2,SIZE2,MN,N>& M2);
	template <typename T1,int SIZE1,typename T2,int SIZE2> inline SymMatrix<N>& add(const Matrix_Base<T1,SIZE1,N,N>& M1,const Matrix_Base<T2,SIZE2,N,N>& M2);
	template <typename T1,int SIZE1,typename T2,int SIZE2> inline SymMatrix<N>& subst(const Matrix_Base<T1,SIZE1,N,N>& M1,const Matrix_Base<T2,SIZE2,N,N>& M2);
	template <typename T1,int SIZE1> inline SymMatrix<N>& add(const Matrix_Base<T1,SIZE1,N,N>& M1);
	template <typename T1,int SIZE1> inline SymMatrix<N>& subst(const Matrix_Base<T1,SIZE1,N,N>& M1);
	template <typename TT,int SSIZE> inline SymMatrix<N>& mult(const Matrix_Base<TT,SSIZE,N,N>& Two);
	template <typename TT,int SSIZE> inline SymMatrix<N>& div(const Matrix_Base<TT,SSIZE,N,N>& Two);
	inline reel trace()const;
	inline SymMatrix<N> inv()const;
	inline SymMatrix<N> inv(reel& determ)const;
#if USE_MKL || USE_ACML
	inline SymMatrix<N> pinv2(double reltol); // attention, modifie *this !!
	inline SymMatrix<N> pinv(double reltol)const {SymMatrix<N> a=*this; return a.pinv2(reltol);}
#endif
	inline reel det()const;
	inline SymMatrix<N>& adddiag(const reel& x);
	inline SymMatrix<N> tred2(Vecteur<N>& d,Vecteur<N>& e);
	inline SquareMatrix<N> tqli(Vecteur<N>& d, Vecteur<N>& e);
	inline SquareMatrix<N> eigenVV(Vecteur<N>& eigenValues) const;
};
template <int N> inline reel& SymMatrix<N>::operator()(const int idxm,const int idxn)
{
	assert((idxm>=0)&&(idxm<N)&&(idxn>=0)&&(idxn<N));
	SymMatrix<N>& ref=*this;
	if (idxm<=idxn)
		return ref[((idxm*((N<<1)-idxm-1))>>1)+idxn];
	else
		return ref[((idxn*((N<<1)-idxn-1))>>1)+idxm];
}
template <int N> inline const reel& SymMatrix<N>::operator()(const int idxm,const int idxn) const
{
	assert((idxm>=0)&&(idxm<N)&&(idxn>=0)&&(idxn<N));
	const SymMatrix<N>& ref=*this;
	if (idxm<=idxn)
		return ref[((idxm*((N<<1)-idxm-1))>>1)+idxn];
	else
		return ref[((idxn*((N<<1)-idxn-1))>>1)+idxm];
}
template <int N> template <typename T1,int SIZE1,int MN,typename T2,int SIZE2> inline SymMatrix<N>& SymMatrix<N>::prod(const Matrix_Base<T1,SIZE1,N,MN>& M1,const Matrix_Base<T2,SIZE2,MN,N>& M2) {
	SymMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=i;j<N;j++) {
			ref(i,j)=0;
			for (int k=0;k<MN;k++)
				ref(i,j)+=M1(i,k)*M2(k,j);
		}
		return ref;
}
template <int N> template <typename T1,int SIZE1,typename T2,int SIZE2> inline SymMatrix<N>& SymMatrix<N>::add(const Matrix_Base<T1,SIZE1,N,N>& M1,const Matrix_Base<T2,SIZE2,N,N>& M2) {
	SymMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=i;j<N;j++)
			ref(i,j)=M1(i,j)+M2(i,j);
	return ref;
}

template <int N> template <typename T1,int SIZE1,typename T2,int SIZE2> inline SymMatrix<N>& SymMatrix<N>::subst(const Matrix_Base<T1,SIZE1,N,N>& M1,const Matrix_Base<T2,SIZE2,N,N>& M2) {
	SymMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=i;j<N;j++)
			ref(i,j)=M1(i,j)-M2(i,j);
	return ref;
}
template <int N> template <typename T1,int SIZE1> inline SymMatrix<N>& SymMatrix<N>::add(const Matrix_Base<T1,SIZE1,N,N>& M1) {
	SymMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=i;j<N;j++)
			ref(i,j)+=M1(i,j);
	return ref;
}

template <int N> template <typename T1,int SIZE1> inline SymMatrix<N>& SymMatrix<N>::subst(const Matrix_Base<T1,SIZE1,N,N>& M1) {
	SymMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=i;j<N;j++)
			ref(i,j)-=M1(i,j);
	return ref;
}
template <int N> template <typename TT,int SSIZE> inline SymMatrix<N>& SymMatrix<N>::mult(const Matrix_Base<TT,SSIZE,N,N>& Two) {
	SymMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=i;j<N;j++)
			ref(i,j)*=Two(i,j);
	return ref;
}
template <int N> template <typename TT,int SSIZE> inline SymMatrix<N>& SymMatrix<N>::div(const Matrix_Base<TT,SSIZE,N,N>& Two) {
	SymMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=i;j<N;j++) {
			assert(Two(i,j)!=0);
			ref(i,j)/=Two(i,j);
		}
	return ref;
}

template <int N> inline reel SymMatrix<N>::trace()const
{
	const SymMatrix<N>& ref=*this;
	reel res(0);
	for (int i=0;i<N;i++)
		res+=ref(i,i);
	return res;
}
template <> inline SymMatrix<1> SymMatrix<1>::inv()const
{
	const SymMatrix<1>& ref=*this;
	SymMatrix<1> res;
	assert(ref(0,0)!=0);
	res(0,0)=1/ref(0,0);
	return res;
}
template <> inline SymMatrix<2> SymMatrix<2>::inv()const
{
	const SymMatrix<2>& ref=*this;
	reel determ=ref(0,0)*ref(1,1)-ref(1,0)*ref(0,1);
	assert(determ!=0);
	SymMatrix<2> res;
	res(0,0)=ref(1,1)/determ;
	res(0,1)=-ref(0,1)/determ;
	res(1,1)=ref(0,0)/determ;
	return res;
}
template <> inline SymMatrix<3> SymMatrix<3>::inv()const
{
	reel determ;
	const SymMatrix<3>& ref=*this;
	reel d0=ref(1,1)*ref(2,2)-ref(1,2)*ref(2,1);
	reel d1=ref(1,0)*ref(2,2)-ref(2,0)*ref(1,2);
	reel d2=ref(1,0)*ref(2,1)-ref(1,1)*ref(2,0);
	determ=ref(0,0)*d0-ref(0,1)*d1+ref(0,2)*d2;
	assert(determ!=0);
	reel d=reel(1)/determ;
	reel d4=ref(0,0)*ref(2,2)-ref(2,0)*ref(0,2);
	reel d5=ref(0,0)*ref(2,1)-ref(2,0)*ref(0,1);
	reel d8=ref(0,0)*ref(1,1)-ref(1,0)*ref(0,1);
	SymMatrix<3> res;
	res(0,0)=d0*d;
	res(0,1)=-d1*d;
	res(0,2)=d2*d;
	res(1,1)=d4*d;
	res(1,2)=-d5*d;
	res(2,2)=d8*d;
	return res;
}
template <> inline SymMatrix<4> SymMatrix<4>::inv()const
{
	const SymMatrix<4>& ref=*this;
	reel d1201=ref(1,0)*ref(2,1)-ref(1,1)*ref(2,0);
	reel d1202=ref(1,0)*ref(2,2)-ref(1,2)*ref(2,0);
	reel d1212=ref(1,1)*ref(2,2)-ref(1,2)*ref(2,1);
	reel d1301=ref(1,0)*ref(3,1)-ref(1,1)*ref(3,0);
	reel d1302=ref(1,0)*ref(3,2)-ref(1,2)*ref(3,0);
	reel d1303=ref(1,0)*ref(3,3)-ref(1,3)*ref(3,0);
	reel d1312=ref(1,1)*ref(3,2)-ref(1,2)*ref(3,1);
	reel d1313=ref(1,1)*ref(3,3)-ref(1,3)*ref(3,1);
	reel d2301=ref(2,0)*ref(3,1)-ref(2,1)*ref(3,0);
	reel d2302=ref(2,0)*ref(3,2)-ref(2,2)*ref(3,0);
	reel d2303=ref(2,0)*ref(3,3)-ref(2,3)*ref(3,0);
	reel d2312=ref(2,1)*ref(3,2)-ref(2,2)*ref(3,1);
	reel d2313=ref(2,1)*ref(3,3)-ref(2,3)*ref(3,1); 
	reel d2323=ref(2,2)*ref(3,3)-ref(2,3)*ref(3,2);
	SymMatrix<4> res;
	res(0,0)=(ref(1,1)*d2323-ref(1,2)*d2313+ref(1,3)*d2312);
	res(1,0)=-(ref(1,0)*d2323-ref(1,2)*d2303+ref(1,3)*d2302);
	res(2,0)=(ref(1,0)*d2313-ref(1,1)*d2303+ref(1,3)*d2301);
	res(3,0)=-(ref(1,0)*d2312-ref(1,1)*d2302+ref(1,2)*d2301);
	res(1,1)=(ref(0,0)*d2323-ref(0,2)*d2303+ref(0,3)*d2302);
	res(2,1)=-(ref(0,0)*d2313-ref(0,1)*d2303+ref(0,3)*d2301);
	res(3,1)=(ref(0,0)*d2312-ref(0,1)*d2302+ref(0,2)*d2301);
	res(2,2)=(ref(0,0)*d1313-ref(0,1)*d1303+ref(0,3)*d1301);
	res(3,2)=-(ref(0,0)*d1312-ref(0,1)*d1302+ref(0,2)*d1301);
	res(3,3)=(ref(0,0)*d1212-ref(0,1)*d1202+ref(0,2)*d1201);
	reel determ=ref(0,0)*res(0,0)+ref(0,1)*res(1,0)+ref(0,2)*res(2,0)+ref(0,3)*res(3,0);
	assert(determ!=0);
	for (int i=0; i<4; i++)
		for (int j=0; j<=i; j++)
			res(i,j)/=determ;
	return res;
}
	
template <int N> inline SymMatrix<N> SymMatrix<N>::inv()const
{
	std::cerr<<"matrice nxn : inverse non defini seulement pour n>4 !!!"<<std::endl;
	return this; // une erreur sera generee si cette fonction est compilee
}
template <> inline SymMatrix<3> SymMatrix<3>::inv(reel& determ)const
{
	const SymMatrix<3>& ref=*this;
	reel d0=ref(1,1)*ref(2,2)-ref(1,2)*ref(2,1);
	reel d1=ref(1,0)*ref(2,2)-ref(2,0)*ref(1,2);
	reel d2=ref(1,0)*ref(2,1)-ref(1,1)*ref(2,0);
	determ=ref(0,0)*d0-ref(0,1)*d1+ref(0,2)*d2;
	assert(determ!=0);
	reel d=reel(1)/determ;
	reel d4=ref(0,0)*ref(2,2)-ref(2,0)*ref(0,2);
	reel d5=ref(0,0)*ref(2,1)-ref(2,0)*ref(0,1);
	reel d8=ref(0,0)*ref(1,1)-ref(1,0)*ref(0,1);
	SymMatrix<3> res;
	res(0,0)=d0*d;
	res(0,1)=-d1*d;
	res(0,2)=d2*d;
	res(1,1)=d4*d;
	res(2,1)=-d5*d;
	res(2,2)=d8*d;
	return res;
}
template <int N> inline SymMatrix<N> SymMatrix<N>::inv(reel& determ)const
{
	std::cerr<<"matrice non 3x3 !!!"<<std::endl;
	return *this; // une erreur sera generee si cette fonction est compilee
}
template <> inline reel SymMatrix<1>::det()const
{
	const SymMatrix<1>& ref=*this;
	return ref(0,0);
}
template <> inline reel SymMatrix<2>::det()const
{
	const SymMatrix<2>& ref=*this;
	return ref(0,0)*ref(1,1)-ref(0,1)*ref(1,0);
}
template <> inline reel SymMatrix<3>::det()const
{
	const SymMatrix<3>& ref=*this;
	reel d0=ref(1,1)*ref(2,2)-ref(1,2)*ref(2,1);
	reel d1=ref(1,0)*ref(2,2)-ref(2,0)*ref(1,2);
	reel d2=ref(1,0)*ref(2,1)-ref(1,1)*ref(2,0);
	return ref(0,0)*d0-ref(0,1)*d1+ref(0,2)*d2;
}
template <> inline reel SymMatrix<4>::det()const
{
	const SymMatrix<4>& ref=*this;
	reel d01=ref(2,2)*ref(3,3)-ref(2,3)*ref(3,2);
	reel d02=ref(2,1)*ref(3,3)-ref(2,3)*ref(3,1);
	reel d03=ref(2,1)*ref(3,2)-ref(2,2)*ref(3,1);
	reel d12=ref(2,0)*ref(3,3)-ref(2,3)*ref(3,0);
	reel d13=ref(2,0)*ref(3,2)-ref(2,2)*ref(3,0);
	reel d23=ref(2,0)*ref(3,1)-ref(2,1)*ref(3,0);
	reel d0=ref(1,1)*d01-ref(1,2)*d02+ref(1,3)*d03;
	reel d1=ref(1,0)*d01-ref(1,2)*d12+ref(1,3)*d13;
	reel d2=ref(1,0)*d02-ref(1,1)*d12+ref(1,3)*d23;
	reel d3=ref(1,0)*d03-ref(1,1)*d13+ref(1,2)*d23;
	return ref(0,0)*d0-ref(0,1)*d1+ref(0,2)*d2-ref(0,3)*d3;
}
template <int N> inline reel SymMatrix<N>::det()const
{
	std::cerr<<"matrice nxn : determinant non defini seulement pour n>4 !!!"<<std::endl;
	return this; // une erreur sera generee si cette fonction est compilee
}

#if USE_MKL || USE_ACML
template<int N> inline SymMatrix<N> SymMatrix<N>::pinv2(double reltol) {
	// svd
	reel evalues[N];
	SquareMatrix<N> evectors;
	reel work[3*N];
	int info;
	int n=N;
#if USE_MKL
	dspev("V","L",&n,this->Tab,evalues,&evectors(0,0),&n,work,&info);
#elif USE_ACML
	dspev_("V","L",&n,this->Tab,evalues,&evectors(0,0),&n,work,&info);
#endif
	// result = evectors' * pinv(diag(evalues)) * evectors
	SquareMatrix<N> left;
	double tol=max(-evalues[0],evalues[N-1])*reltol;
	for (int i=0; i<N; i++) {
		double d = evalues[i];
		if (d>=tol) for (int j=0; j<N; j++) left(j,i)=evectors(i,j)/d;
		else for (int j=0; j<N; j++) left(i,j)=0;
	}
	SymMatrix<N> res;
	res.prod(left,evectors);
	return res;
}
#endif

template <int N> inline SymMatrix<N>& SymMatrix<N>::adddiag(const reel& x)
{
	SymMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		ref(i,i)+=x;
	return ref;
}

template <int N> inline SymMatrix<N> SymMatrix<N>::tred2(Vecteur<N>& d,Vecteur<N>& e)
{
	int l,k,j,i;
	reel scale,hh,h,g,f;
	SymMatrix<N>& local=*this;
	SquareMatrix<N> a;
	for (i=0;i<N;i++)
	{
		a(i,i)=local(i,i);
		for (int j=0;j<i;j++)
		{
			a(i,j)=a(j,i)=local(i,j);
		}
	}
	for (i=N-1;i>=1;i--) {
		l=i-1;
		h=scale=0.0;
		if (l > 0) {
			for (k=0;k<=l;k++)
				scale += fabs(a(i,k));
			if (scale == 0.0)
				e(i)=a(i,l);
			else {
				for (k=0;k<=l;k++) {
					a(i,k) /= scale;
					h += a(i,k)*a(i,k);
				}
				f=a(i,l);
				g=(f >= 0.0 ? -::sqrt(h) : ::sqrt(h));
				e(i)=scale*g;
				h -= f*g;
				a(i,l)=f-g;
				f=0.0;
				for (j=0;j<=l;j++) {
					a(j,i)=a(i,j)/h;
					g=0.0;
					for (k=0;k<=j;k++)
						g += a(j,k)*a(i,k);
					for (k=j+1;k<=l;k++)
						g += a(k,j)*a(i,k);
					e(j)=g/h;
					f += e(j)*a(i,j);
				}
				hh=f/(h+h);
				for (j=0;j<=l;j++) {
					f=a(i,j);
					e(j)=g=e(j)-hh*f;
					for (k=0;k<=j;k++)
						a(j,k) -= (f*e(k)+g*a(i,k));
				}
			}
		} else
			e(i)=a(i,l);
		d(i)=h;
	}
	d(0)=0.0;
	e(0)=0.0;
	// Contents of this loop can be omitted if eigenvectors not
	//		wanted except for statement d(i)=a(i,i); 
	for (i=0;i<=N-1;i++) {
		l=i-1;
		if (d(i)) {
			for (j=0;j<=l;j++) {
				g=0.0;
				for (k=0;k<=l;k++)
					g += a(i,k)*a(k,j);
				for (k=0;k<=l;k++)
					a(k,j) -= g*a(k,i);
			}
		}
		d(i)=a(i,i);
		a(i,i)=1.0;
		for (j=0;j<=l;j++) a(j,i)=a(i,j)=0.0;
	}
	for (i=0;i<N;i++)
	{
		local(i,i)=a(i,i);
		for (int j=0;j<i;j++)
		{
			local(i,j)=a(i,j);
		}
	}
	return local;
}

template <int N> inline SquareMatrix<N> SymMatrix<N>::tqli(Vecteur<N>& d, Vecteur<N>& e)
{
	int m,l,iter,i,k;
	reel s,r,p,g,f,dd,c,b;
	SquareMatrix<N> res;
	SymMatrix<N>& local=*this;
	//recopie de this dans res
	for (i=0;i<N;i++)
	{
		res(i,i)=local(i,i);
		for (int j=0;j<i;j++)
		{
			res(i,j)=local(i,j);
			res(j,i)=local(i,j);
		}
	}

	for (i=1;i<N;i++) e[i-1]=e[i];
	e[N-1]=0.0;
	for (l=0;l<N;l++) {
		iter=0;
		do {
			for (m=l;m<N-1;m++) {
				dd=fabs(d[m])+fabs(d[m+1]);
				if (fabs(e[m])+dd == dd) break;
			}
			if (m != l) {
				if (iter++ == 30) { std::cout<<"Too many iterations in tqli"<<std::endl; return res; }
				g=(d[l+1]-d[l])/(reel(2.0)*e[l]);
				r=pythag(g,1.0);
				g=d[m]-d[l]+e[l]/(g+(g >= 0 ? (r >= 0 ? r : -r) : (r >= 0 ? -r : r)));
				s=c=1.0;
				p=0.0;
				for (i=m-1;i>=l;i--) {
					f=s*e[i];
					b=c*e[i];
					e[i+1]=(r=pythag(f,g));
					if (r == 0.0) {
						d[i+1] -= p;
						e[m]=0.0;
						break;
					}
					s=f/r;
					c=g/r;
					g=d[i+1]-p;
					r=(d[i]-g)*s+reel(2.0)*c*b;
					d[i+1]=g+(p=s*r);
					g=c*r-b;
					// Next loop can be omitted if eigenvectors not wanted
					for (k=0;k<N;k++) {
						f=res(k,i+1);
						res(k,i+1)=s*res(k,i)+c*f;
						res(k,i)=c*res(k,i)-s*f;
					}
				}
				if (r == 0.0 && i >= l) continue;
				d[l] -= p;
				e[l]=g;
				e[m]=0.0;
			}
		} while (m != l);
	}
	return res;
}


template <int N> inline SquareMatrix<N> SymMatrix<N>::eigenVV(Vecteur<N>& eigenValues)const
{
	Vecteur<N> temp;
	//glauque
	SymMatrix<3> tempo=*this;
	return tempo.tred2(eigenValues,temp).tqli(eigenValues,temp);
}

NAMESPACE_END

#endif
